#include <Matrix_ESP32.h>

Matrix_ESP32::Matrix_ESP32(uint8_t mcp_Addr):Adafruit_MCP23017(mcp_Addr)
{
	
}

void Matrix_ESP32::init()
{
	begin(0x00);

	//声明电机引脚为output
	Adafruit_MCP23017::pinMode(M1DIR1, OUTPUT);
	Adafruit_MCP23017::pinMode(M1DIR2, OUTPUT);
	Adafruit_MCP23017::pinMode(M2DIR1, OUTPUT);
	Adafruit_MCP23017::pinMode(M2DIR2, OUTPUT);
	Adafruit_MCP23017::pinMode(M3DIR1, OUTPUT);
	Adafruit_MCP23017::pinMode(M3DIR2, OUTPUT);
	Adafruit_MCP23017::pinMode(M4DIR1, OUTPUT);
	Adafruit_MCP23017::pinMode(M4DIR2, OUTPUT);

	//PWM通道分辨率
	::ledcSetup(12, 2000, 8);
	::ledcSetup(13, 2000, 8);
	::ledcSetup(14, 2000, 8);
	::ledcSetup(15, 2000, 8);

	//PWM引脚关联
	::ledcAttachPin(M1PWM, 12);
	::ledcAttachPin(M2PWM, 13);
	::ledcAttachPin(M3PWM, 14);
	::ledcAttachPin(M4PWM, 15);

}

void Matrix_ESP32::motorRun(uint8_t No, int speed, int direction = 1)
{
	if ((direction != 1) && (direction != -1)) {
		direction = 0;
	}

	if (speed < 0) {
		speed = 0;
	} else if (speed > 255) {
		speed = 255;
	}

	speed = speed * direction;

	bool dir1, dir2;
	int _speed = abs(speed);

	if (speed == 0) {
		dir1 = false;
		dir2 = false;
	} else if (speed < 0) {
		dir1 = true;
		dir2 = false;
	} else {
		dir1 = false;
		dir2 = true;
	}
		
	switch(No) {
		case 1:
			Adafruit_MCP23017::digitalWrite(M1DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M1DIR2, dir2);
			::ledcWrite(12, _speed);
			break;
		case 2:
			Adafruit_MCP23017::digitalWrite(M2DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M2DIR2, dir2);
			::ledcWrite(13, _speed);
			break;
		case 3:
			Adafruit_MCP23017::digitalWrite(M3DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M3DIR2, dir2);
			::ledcWrite(14, _speed);
			break;
		case 4:
			Adafruit_MCP23017::digitalWrite(M4DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M4DIR2, dir2);
			::ledcWrite(15, _speed);
			break;
	}
	
}

void Matrix_ESP32::motorRun(uint8_t No, int speed)
{
	if (speed < -255) {
		speed = -255;
	} else if (speed > 255) {
		speed = 255;
	}

	bool dir1, dir2;
	int _speed = abs(speed);

	if (speed == 0) {
		dir1 = false;
		dir2 = false;
	} else if (speed < 0) {
		dir1 = true;
		dir2 = false;
	} else {
		dir1 = false;
		dir2 = true;
	}
		
	switch(No) {
		case 1:
			Adafruit_MCP23017::digitalWrite(M1DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M1DIR2, dir2);
			::ledcWrite(12, _speed);
			break;
		case 2:
			Adafruit_MCP23017::digitalWrite(M2DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M2DIR2, dir2);
			::ledcWrite(13, _speed);
			break;
		case 3:
			Adafruit_MCP23017::digitalWrite(M3DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M3DIR2, dir2);
			::ledcWrite(14, _speed);
			break;
		case 4:
			Adafruit_MCP23017::digitalWrite(M4DIR1, dir1);
			Adafruit_MCP23017::digitalWrite(M4DIR2, dir2);
			::ledcWrite(15, _speed);
			break;
	}

}

void Matrix_ESP32::motorStop(uint8_t No)
{		
	switch(No){
		case 1:
			Adafruit_MCP23017::digitalWrite(M1DIR1, true);
			Adafruit_MCP23017::digitalWrite(M1DIR2, true);
			::ledcWrite(12, 0);
			break;
		case 2:
			Adafruit_MCP23017::digitalWrite(M2DIR1, true);
			Adafruit_MCP23017::digitalWrite(M2DIR2, true);
			::ledcWrite(13, 0);
			break;
		case 3:
			Adafruit_MCP23017::digitalWrite(M3DIR1, true);
			Adafruit_MCP23017::digitalWrite(M3DIR2, true);
			::ledcWrite(14, 0);
			break;
		case 4:
			Adafruit_MCP23017::digitalWrite(M4DIR1, true);
			Adafruit_MCP23017::digitalWrite(M4DIR2, true);
			::ledcWrite(15, 0);
			break;
	}
	
}

bool Matrix_ESP32::getLineSensorState(uint8_t data)
{
	uint8_t val=0;
	for(int8_t i = 4;i>=0;i--)
		val = (val<<1)|(Adafruit_MCP23017::digitalRead(i));
	return data==val;
}